#!/usr/bin/env python3
import rospy
from std_msgs.msg import Int32MultiArray

def publish_path():
    pub = rospy.Publisher('path_topic', Int32MultiArray, queue_size=10)
    rospy.init_node('path_simulator', anonymous=True)
    rospy.sleep(1.0)  # 等待订阅者连接

    # 定义路径点 (x, y)
    path_xy = [
    (0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (0, 5), (0, 6), (0, 7), (0, 8),
    (1, 8), (1, 7), (1, 6), (1, 5), (1, 4), (1, 3), (1, 2), (1, 1), (1, 0),
    (2, 0), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8),
    (3, 8), (3, 7), (3, 6), (3, 5), (3, 4), (3, 3), (3, 2), (3, 1), (3, 0),
    (4, 0), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (4, 6), (5, 6), (5, 5),
    (5, 4), (5, 3), (5, 2), (5, 1), (5, 0), (6, 0), (6, 1), (6, 2), (6, 3),
    (6, 4), (6, 5), (6, 6), (5, 6), (4, 6), (3, 6), (3, 7), (3, 8), (4, 8),
    (5, 8), (6, 8), (5, 8), (4, 8), (3, 8), (2, 8), (1, 8), (0, 8), (0, 7),
    (0, 6), (0, 5), (0, 4), (0, 3), (0, 2), (0, 1), (0, 0)
]

    # 构造数据帧
    data = [-2, -2]
    for x, y in path_xy:
        data.extend([x, y])
    data.extend([-9, -9])

    msg = Int32MultiArray(data=data)
    pub.publish(msg)
    rospy.loginfo("Published path with %d waypoints (%d integers)", len(path_xy), len(data))

if __name__ == '__main__':
    try:
        publish_path()
    except rospy.ROSInterruptException:
        pass

